#include <stdlib.h>

#include "middleware/tarm_tim.h"
#include "middleware/tarm_math.h"
#include "middleware/tarm_exit.h"
#include "middleware/tarm_uart.h"

#include "hardware/tarm_io.h"
#include "hardware/tarm_limit.h"
#include "hardware/tarm_pwm.h"
#include "hardware/tarm_baseTimer.h"

#include "application/tarm_init.h"
#include "application/tarm_run.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>

#include "hardware/closeloop/CloseLoop.h"


/************ function **************/
//******************************
//@brief 		闭环反馈
//@param[in]
//@return
//******************************
void CloseLoopUpdate()
{
	if(CloseLoop_Gather() != 1){
		PRINT_MAIN("无闭环：error");
		return;
	}

//	PRINT_MAIN("闭环结果：\tprime:%f;\r\n\t\tvice:%f",CloseLoopSingle.prime, CloseLoopSingle.vice);

	g_TARM_RunRecord.pv_current.p = CloseLoopSingle.prime;
	g_TARM_RunRecord.pv_current.v = CloseLoopSingle.vice;
	angle_cur_to_xy();

	PRINT_MAIN("\nClose Loop Update\n");

	ReportStruct.Uart1ReportFlag.CloseLoopReport = 1;
	ReportStruct.Uart2ReportFlag.CloseLoopReport = 1;
}

/************************************************************
*	复位函数，初始化、碰到限位时使用
*	将目标、现时的xy、angle值全部复位
	除能TIM2、3中断
************************************************************/
//void reset_point(void)
//{
//	g_TARM_RunRecord.pv_distance.p = g_TARM_Factor.limit_p;
//	g_TARM_RunRecord.pv_current.p = g_TARM_Factor.limit_p;
//	g_TARM_RunRecord.pv_distance.v = g_TARM_Factor.limit_v;
//	g_TARM_RunRecord.pv_current.v = g_TARM_Factor.limit_v;
//
//	angle_des_to_xy();
//	g_TARM_RunRecord.pv_current.x = g_TARM_RunRecord.pv_distance.x;
//	g_TARM_RunRecord.pv_current.y = g_TARM_RunRecord.pv_distance.y;
//
//	//TIM_Cmd(TIM2, DISABLE);
//	//TIM_Cmd(TIM3, DISABLE);
//}

void reset_point_prime(void)
{
	TimeBase_Disable();
	TimePV_Disable();
	g_TARM_RunRecord.RunFlag_p = 0;

	g_TARM_RunRecord.pv_distance.p = g_TARM_Factor.limit_p;
	g_TARM_RunRecord.pv_current.p = g_TARM_Factor.limit_p;
	angle_des_to_xy();
	g_TARM_RunRecord.pv_current.x = g_TARM_RunRecord.pv_distance.x;
	g_TARM_RunRecord.pv_current.y = g_TARM_RunRecord.pv_distance.y;

	PRINT_MAIN("\r\n\tlimit_prime!!!!!!!!!!\r\n");

	Limit_Disable();
	goto_null_point();
	Limit_Enable();
}

void reset_point_vice(void)
{
	TimeBase_Disable();
	TimePV_Disable();
	g_TARM_RunRecord.RunFlag_v = 0;

	g_TARM_RunRecord.pv_distance.v = g_TARM_Factor.limit_v;
	g_TARM_RunRecord.pv_current.v = g_TARM_Factor.limit_v;
	angle_des_to_xy();
	g_TARM_RunRecord.pv_current.x = g_TARM_RunRecord.pv_distance.x;
	g_TARM_RunRecord.pv_current.y = g_TARM_RunRecord.pv_distance.y;

	PRINT_MAIN("\r\n\tlimit_vice!!!!!!!!!!!!\r\n");

	Limit_Disable();
	goto_null_point();
	Limit_Enable();
}

/************************************************************
*	回到零点
*	（null_point）：g_TARM_Factor.null_p=60,g_TARM_Factor.null_v=40;
*	参考点是g_TARM_Factor.limit_p=57,g_TARM_Factor.limit_v=32;
************************************************************/
void goto_null_point(void)
{
	point_to_point_angle(g_TARM_Factor.null_p,g_TARM_Factor.null_v,200);
//	point_to_point_xy_pvthr(17, 12, 150);
}

/************************************************************
*	夹持
************************************************************/
void click_pwm(void)
{
	double duty_cycle = (1200.0 - (double)g_TARM_RunRecord.ClickNum)/1200.0;
	double duty_time = (1200.0 - (double)g_TARM_RunRecord.ClickNum)/1200.0*20;

	PwmClick_ChangeValue(g_TARM_RunRecord.ClickNum);

//	PRINT_MAIN("\n pwm change,占空比：%f;占空时间：%f \r\n", duty_cycle, duty_time);
}

void turn_pwm(void)
{
	double duty_cycle = (1200.0 - (double)g_TARM_RunRecord.ClickNum)/1200.0;
	double duty_time = (1200.0 - (double)g_TARM_RunRecord.ClickNum)/1200.0*20;

	PwmTurn_ChangeValue(g_TARM_RunRecord.TurnNum);

//	PRINT_MAIN("\n pwm change,占空比：%f;占空时间：%f \r\n", duty_cycle, duty_time);
}
